# -*- encoding: utf-8 -*-
'''
@File    :   sync.py
@Time    :   2023/02/13 14:18:25
@Author  :   hgy_rick
@Description : some sync time module
'''

from ..Exception.Python.BasicException import *
import numpy as np

'''
for now, it only support one dimension
src align with dst, dst is reference, after sync
src.size == dst.size, which dst size is const!!
@slerp : function to intepolate, default linear function
'''

def syncDataWithStamp(src_stamp, src, dst_stamp, dst, slerp=None):

    # make sure dst stamp is wrapped by src stamp
    if len(src_stamp) < 10:
        raise NotEnoughDataException(len(src_stamp), 10)

    dst_start = 0
    while dst_stamp[dst_start] < src_stamp[0]:
        dst_start += 1
    dst_end = len(dst_stamp) - 1
    while dst_stamp[dst_end] > src_stamp[-1]:
        dst_end -= 1

    # now wrap
    dst = dst[dst_start:dst_end+1]
    dst_stamp = dst_stamp[dst_start:dst_end+1]
    
    new_src = list()
    src_id = 0
    dst_id = 0

    if slerp == None:
        def slerp(t, value, t0):
            return value[0] + 1.0 * (t0 - t[0]) / (t[1] - t[0]) * (value[1] - value[0])

    while src_id < len(src_stamp)-1:
        if src_stamp[src_id] <= dst_stamp[dst_id] <= src_stamp[src_id+1]:
            # slerp
            y = slerp(src_stamp[src_id:src_id+2], src[src_id:src_id+2], dst_stamp[dst_id])
            new_src.append(y)
            dst_id += 1
            if dst_id >= len(dst_stamp):
                break
        else:
            src_id += 1

    if len(new_src) != len(dst):
        raise ExpectEqualException(len(new_src), len(dst))

    return dst_stamp, new_src, dst
